
class PIDController {
 public:
  PIDController();
  ~PIDController();

  void setKi(double Ki) { _Ki = Ki; }
  void setKp(double Kp) { _Kp = Kp; }
  void setKd(double Kd) { _Kd = Kd; }
  

  void setPosError(double pError) { _pError = pError; }
  void setVelError(double vError) { _vError = vError; }
  void updateController(void);
  double getOutput(void) { return _output; }  

 private:
  double  _Ki;
  double  _Kp;
  double  _Kd;

  double  _pError;
  double  _vError;

  double  _pTerm;
  double  _iTerm;
  double  _dTerm;

  double  _output;
};

inline PIDController::PIDController() {
  _Ki = 0;
  _Kp = 1;
  _Kd = 1;

  _pError = 0;
  _vError = 0;

  _pTerm = 0;
  _iTerm = 0;
  _dTerm = 0;

  _output = 0;
}

inline void PIDController::updateController(void) {
  _pTerm =  _Kp * _pError;
  _iTerm += _Ki * _pError;
  _dTerm =  _Kd * _vError;

  _output = -_pTerm - _iTerm - _dTerm;
}
